extern "C" {
  #include <stdlib.h>
  #include <string.h>
  #include <inttypes.h>
  #include "twi.h"
}

//#include "Stream.h"

#define BUFFER_LENGTH 8

#define MPU6050_ADDRESS_AD0_LOW     0x68 // address pin low (GND), default for InvenSense evaluation board
#define MPU6050_ADDRESS_AD0_HIGH    0x69 // address pin high (VCC)
#define MPU6050_DEFAULT_ADDRESS     MPU6050_ADDRESS_AD0_LOW

#define MPU6050_RA_PWR_MGMT_1       0x6B

#define MPU6050_PWR1_CLKSEL_BIT         2
#define MPU6050_PWR1_CLKSEL_LENGTH      3
#define MPU6050_PWR1_SLEEP_BIT          6

#define MPU6050_ACCEL_FS_2          0x00
#define MPU6050_ACCEL_FS_4          0x01
#define MPU6050_ACCEL_FS_8          0x02
#define MPU6050_ACCEL_FS_16         0x03

#define MPU6050_CLOCK_PLL_XGYRO         0x01

#define MPU6050_RA_GYRO_CONFIG      0x1B

#define MPU6050_GCONFIG_FS_SEL_BIT      4
#define MPU6050_GCONFIG_FS_SEL_LENGTH   2

#define MPU6050_GYRO_FS_250         0x00
#define MPU6050_GYRO_FS_500         0x01
#define MPU6050_GYRO_FS_1000        0x02
#define MPU6050_GYRO_FS_2000        0x03

#define MPU6050_RA_ACCEL_CONFIG     0x1C

#define MPU6050_ACONFIG_AFS_SEL_BIT         4
#define MPU6050_ACONFIG_AFS_SEL_LENGTH      2

#define MPU6050_RA_WHO_AM_I         0x75

#define MPU6050_WHO_AM_I_BIT        6
#define MPU6050_WHO_AM_I_LENGTH     6

#define MPU6050_RA_CONFIG           0x1A

#define MPU6050_RA_ACCEL_XOUT_H     0x3B
#define MPU6050_RA_ACCEL_XOUT_L     0x3C
#define MPU6050_RA_ACCEL_YOUT_H     0x3D
#define MPU6050_RA_ACCEL_YOUT_L     0x3E
#define MPU6050_RA_ACCEL_ZOUT_H     0x3F
#define MPU6050_RA_ACCEL_ZOUT_L     0x40
#define MPU6050_RA_TEMP_OUT_H       0x41
#define MPU6050_RA_TEMP_OUT_L       0x42
#define MPU6050_RA_GYRO_XOUT_H      0x43
#define MPU6050_RA_GYRO_XOUT_L      0x44
#define MPU6050_RA_GYRO_YOUT_H      0x45
#define MPU6050_RA_GYRO_YOUT_L      0x46
#define MPU6050_RA_GYRO_ZOUT_H      0x47
#define MPU6050_RA_GYRO_ZOUT_L      0x48

#define MPU6050_RA_SMPLRT_DIV       0x19

#define MPU6050_CFG_EXT_SYNC_SET_BIT    5
#define MPU6050_CFG_EXT_SYNC_SET_LENGTH 3
#define MPU6050_CFG_DLPF_CFG_BIT    2
#define MPU6050_CFG_DLPF_CFG_LENGTH 3

uint8_t rxBuffer[BUFFER_LENGTH];
uint8_t rxBufferIndex = 0;
uint8_t rxBufferLength = 0;

uint8_t txAddress = 0;
uint8_t txBuffer[BUFFER_LENGTH];
uint8_t txBufferIndex = 0;
uint8_t txBufferLength = 0;

uint8_t transmitting = 0;


void begin(void)
{
  rxBufferIndex = 0;
  rxBufferLength = 0;

  txBufferIndex = 0;
  txBufferLength = 0;

  twi_init();
}


void beginTransmission(uint8_t address)
{
  // indicate that we are transmitting
  transmitting = 1;
  // set address of targeted slave
  txAddress = address;
  // reset tx buffer iterator vars
  txBufferIndex = 0;
  txBufferLength = 0;
}


// must be called in:
// slave tx event callback
// or after beginTransmission(address)
size_t write(uint8_t data)
{
  if(transmitting){
  // in master transmitter mode
    // don't bother if buffer is full
    // put byte in tx buffer
    txBuffer[txBufferIndex] = data;
    ++txBufferIndex;
    // update amount in buffer   
    txBufferLength = txBufferIndex;
  }
  return 1;
}

uint8_t endTransmission()
{
  // transmit buffer (blocking)
  int8_t ret = twi_writeTo(txAddress, txBuffer, txBufferLength, 1, true);
  // reset tx buffer iterator vars
  txBufferIndex = 0;
  txBufferLength = 0;
  // indicate that we are done transmitting
  transmitting = 0;
  return ret;
}



uint8_t requestFrom(uint8_t address, uint8_t quantity)
{
  // perform blocking read into buffer
  uint8_t read = twi_readFrom(address, rxBuffer, quantity, 1);
  // set rx buffer iterator vars
  rxBufferIndex = 0;
  rxBufferLength = read;

  return read;
}

int available(void)
{
  return rxBufferLength - rxBufferIndex;
}

int read(void)
{
  int value = -1;
  
  // get each successive byte on each call
  if(rxBufferIndex < rxBufferLength){
    value = rxBuffer[rxBufferIndex];
    ++rxBufferIndex;
  }

  return value;
}


void writeBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t* data) {
    uint8_t status = 0;
    beginTransmission(devAddr);
    write((uint8_t) regAddr); // send address
    for (uint8_t i = 0; i < length; i++) {
        write((uint8_t) data[i]);
    }
    endTransmission();
}


int8_t readBytes(uint8_t devAddr, uint8_t regAddr, uint8_t length, uint8_t *data) {
    int8_t count = 0;

    beginTransmission(devAddr);
    write(regAddr);
    endTransmission();

    beginTransmission(devAddr);
    requestFrom(devAddr, length);

    for (; available(); count++) {
        data[count] = read();
    }

    endTransmission();

    return count;
}

void writeByte(uint8_t devAddr, uint8_t regAddr, uint8_t data) {
    writeBytes(devAddr, regAddr, 1, &data);
}


int8_t readByte(uint8_t devAddr, uint8_t regAddr, uint8_t *data) {
    return readBytes(devAddr, regAddr, 1, data);
}

int8_t readBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t *data) {
    // 01101001 read byte
    // 76543210 bit numbers
    //    xxx   args: bitStart=4, length=3
    //    010   masked
    //   -> 010 shifted
    uint8_t count, b;
    if ((count = readByte(devAddr, regAddr, &b)) != 0) {
        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
        b &= mask;
        b >>= (bitStart - length + 1);
        *data = b;
    }
    return count;
}

void writeBits(uint8_t devAddr, uint8_t regAddr, uint8_t bitStart, uint8_t length, uint8_t data) {
    uint8_t b;
    if (readByte(devAddr, regAddr, &b) != 0) {
        uint8_t mask = ((1 << length) - 1) << (bitStart - length + 1);
        data <<= (bitStart - length + 1);
        data &= mask;
        b &= ~(mask);
        b |= data;
        writeByte(devAddr, regAddr, b);
    }
}

void writeBit(uint8_t devAddr, uint8_t regAddr, uint8_t bitNum, uint8_t data) {
    uint8_t b;
    readByte(devAddr, regAddr, &b);
    b = (data != 0) ? (b | (1 << bitNum)) : (b & ~(1 << bitNum));
    writeByte(devAddr, regAddr, b);
}


uint8_t devAddr = MPU6050_DEFAULT_ADDRESS;

void setFullScaleGyroRange(uint8_t range) {
    writeBits(devAddr, MPU6050_RA_GYRO_CONFIG, MPU6050_GCONFIG_FS_SEL_BIT, MPU6050_GCONFIG_FS_SEL_LENGTH, range);
}

void setClockSource(uint8_t source) {
    writeBits(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_CLKSEL_BIT, MPU6050_PWR1_CLKSEL_LENGTH, source);
}

void setFullScaleAccelRange(uint8_t range) {
    writeBits(devAddr, MPU6050_RA_ACCEL_CONFIG, MPU6050_ACONFIG_AFS_SEL_BIT, MPU6050_ACONFIG_AFS_SEL_LENGTH, range);
}

void setSleepEnabled(bool enabled) {
    writeBit(devAddr, MPU6050_RA_PWR_MGMT_1, MPU6050_PWR1_SLEEP_BIT, enabled);
}

void initialize() {
    setClockSource(MPU6050_CLOCK_PLL_XGYRO);
    setFullScaleGyroRange(MPU6050_GYRO_FS_500);//MPU6050_GYRO_FS_2000   MPU6050_GYRO_FS_250
    setFullScaleAccelRange(MPU6050_ACCEL_FS_2);
    setSleepEnabled(false); // thanks to Jack Elston for pointing this one out!
}

uint8_t getDeviceID() {
    uint8_t buff[1];
    readBits(devAddr, MPU6050_RA_WHO_AM_I, MPU6050_WHO_AM_I_BIT, MPU6050_WHO_AM_I_LENGTH, buff);
    return buff[0];
}

void setRate(uint8_t rate) {
    writeByte(devAddr, MPU6050_RA_SMPLRT_DIV, rate);
}

void setDLPFMode(uint8_t mode) {
    writeBits(devAddr, MPU6050_RA_CONFIG, MPU6050_CFG_DLPF_CFG_BIT, MPU6050_CFG_DLPF_CFG_LENGTH, mode);
}


#define MPU6050_RA_USER_CTRL        0x6A
#define MPU6050_USERCTRL_FIFO_EN_BIT            6
void setFIFOEnabled(bool enabled) {
    writeBit(devAddr, MPU6050_RA_USER_CTRL, MPU6050_USERCTRL_FIFO_EN_BIT, enabled);
}

#define MPU6050_RA_FIFO_EN          0x23
#define MPU6050_TEMP_FIFO_EN_BIT    7
#define MPU6050_XG_FIFO_EN_BIT      6
#define MPU6050_YG_FIFO_EN_BIT      5
#define MPU6050_ZG_FIFO_EN_BIT      4
#define MPU6050_ACCEL_FIFO_EN_BIT   3
#define MPU6050_SLV2_FIFO_EN_BIT    2
#define MPU6050_SLV1_FIFO_EN_BIT    1
#define MPU6050_SLV0_FIFO_EN_BIT    0

void setXGyroFIFOEnabled(bool enabled) {
    writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_XG_FIFO_EN_BIT, enabled);
}
void setYGyroFIFOEnabled(bool enabled) {
    writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_YG_FIFO_EN_BIT, enabled);
}
void setZGyroFIFOEnabled(bool enabled) {
    writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ZG_FIFO_EN_BIT, enabled);
}
void setAccelFIFOEnabled(bool enabled) {
    writeBit(devAddr, MPU6050_RA_FIFO_EN, MPU6050_ACCEL_FIFO_EN_BIT, enabled);
}



bool testConnection() {
    return getDeviceID() == 0b110100;
}



//uint8_t buffer[14*16];

//char arr[] = {'0', '1', '2', '3', '4', '5', '6', '7', '8', '9', 'A', 'B', 'C', 'D', 'E', 'F'};
//void printByte(uint8_t val){
//  Serial.print(arr[(val&0xF0)>>4]);
//  Serial.print(arr[(val&0x0F)]);
//  Serial.print(' ');
//}
uint8_t doCalib;
uint8_t calib_status;


void setup()
{
  begin(); // join i2c bus (address optional for master)
  Serial.begin(19200);
  initialize();
  //Serial.println(testConnection() ? "MPU6050 connection successful" : "MPU6050 connection failed");
  //setRate(7);
  setRate(0);
  setDLPFMode(1);
  
  //setFIFOEnabled(1);
  //setXGyroFIFOEnabled(1);
  //setYGyroFIFOEnabled(1);
  //setZGyroFIFOEnabled(1);
  //setAccelFIFOEnabled(1);

  TCCR1A = 0;
  TCCR1B = 3;// 1/64 =>  4us/tick(250tick/ms)  total 262144 us 1/4s
  //TCCR1B = 4;// 1/256 =>  16us/tick(62.5tick/ms)  total 1048576 us 1s
  //TCCR1B = 5;// 1/1024 =>  64us/tick  total 4194304 us 4s
  
  TCCR0A = 3;
  TCCR0B = 3 | 8;// 1/64 =>  4us/tick(250tick/ms)  total ~1ms
  //TCCR0B = 4 | 8;// 1/256 =>  16us/tick(62.5tick/ms)  total 4096us  4.096ms
  //TCCR0B = 5 | 8;// 1/1024 =>  64us/tick 16384us, total 16ms
  //TIMSK0 = 1;
  TIMSK0 = _BV(1);
  
  OCR0A = 250; // 1ms /sample
  
  
//   PCICR |= _BV(PCIE1);
//   //GIMSK |= _BV(PCIE0);
//   PCMSK1 |= _BV(0);//start button
//   PCMSK1 |= _BV(1);//start button
//   PCMSK1 |= _BV(2);//start button 前小 后大
//   PCMSK1 |= _BV(3);//start button
  PCICR |= (1 << PCIE0);   //enable PCMSK0 scan
  PCMSK0 |= (1 << PCINT0); //set pin 8 for detecting state change
  PCMSK0 |= (1 << PCINT1); //set pin 9 for detecting state change
  PCMSK0 |= (1 << PCINT2); //set pin 10 for detecting state change
  PCMSK0 |= (1 << PCINT3); //set pin 11 for detecting state change
  PCMSK0 |= (1 << PCINT4); //set pin 12 for detecting state change


  PINB |= _BV(4);//temp
  
  calib_status = 0;
  
  //DDRD = _BV(2) | _BV(3) | _BV(4) | _BV(5);
  DDRD |= B11110000;

  switchProc(1);
  //PCIFR PCIF2
  sei();
}

//byte val = 0;

volatile uint16_t time1;//mode
volatile uint16_t time2;//roll
volatile uint16_t time3;//yaw
volatile uint16_t time4;//pitch
volatile uint16_t time5;//throttle

uint16_t oldtimer1;
uint16_t oldtimer2;
uint16_t oldtimer3;
uint16_t oldtimer4;
uint16_t oldtimer5;
uint8_t oldpinval;
ISR(PCINT0_vect){
  uint8_t pinval = PINB;
  uint16_t time = TCNT1;
  if((pinval & _BV(0)) && !(oldpinval & _BV(0))){
    oldtimer1 = time;
  }
  if(!(pinval & _BV(0)) && (oldpinval & _BV(0))){
    time1 =  time - oldtimer1;
  }
  
  if((pinval & _BV(1)) && !(oldpinval & _BV(1))){
    oldtimer2 = time;
  }
  if(!(pinval & _BV(1)) && (oldpinval & _BV(1))){
    time2 =  time - oldtimer2;
  }
  
  if((pinval & _BV(2)) && !(oldpinval & _BV(2))){
    oldtimer3 = time;
  }
  if(!(pinval & _BV(2)) && (oldpinval & _BV(2))){
    time3 =  time - oldtimer3;
  }
  
  if((pinval & _BV(3)) && !(oldpinval & _BV(3))){
    oldtimer4 = time;
  }
  if(!(pinval & _BV(3)) && (oldpinval & _BV(3))){
    time4 =  time - oldtimer4;
  }
  
  if((pinval & _BV(4)) && !(oldpinval & _BV(4))){
    oldtimer5 = time;
  }
  if(!(pinval & _BV(4)) && (oldpinval & _BV(4))){
    time5 =  time - oldtimer5;
  }
  oldpinval = pinval;
}

uint16_t calib_count;

int32_t calib_x;
int32_t calib_y;
int32_t calib_z;
int32_t calib_ax;
int32_t calib_ay;

int32_t inc_x;
int32_t inc_y;
int32_t inc_z;

int16_t ax, ay, az;
uint8_t t0;
uint8_t ff;

int16_t gx, gy, gz;


//wiring.c 注释掉 ISR(TIMER0_OVF_vect)
//ISR(TIMER0_OVF_vect){
ISR(TIMER0_COMPA_vect){
  //计算上一次采样的结果
  uint8_t index = getcurrentIndex();
  uint8_t* buff = getBuff();
  uint8_t status = getstatus();
  
  buff = &buff[index*14];
  ax = (int16_t)buff[0] << 8;
  ax |= buff[1];
  
  ay = (int16_t)buff[2] << 8;
  ay |= buff[3];

  az = (int16_t)buff[4] << 8;
  az |= buff[5];//TODO 用数组平均一下，取delta

  t0 = buff[6];
  ff = status;
  
  gx = (int16_t)buff[8] << 8;
  gx |= buff[9];
  
  gy = (int16_t)buff[10] << 8;
  gy |= buff[11];

  gz = (int16_t)buff[12] << 8;
  gz |= buff[13];

  if(doCalib){//后拨 校准 
    if(calib_status==0){
      
      calib_count = 0;
      calib_x = gx;
      calib_y = gy;
      calib_z = gz;
      calib_ax = ax;
      calib_ay = ay;

      inc_x = 0;
      inc_y = 0;
      inc_z = 0;

      calib_status=1;
    }else{
      if(calib_count<1024){
        calib_count++;
        calib_x += gx;
        calib_y += gy;
        calib_z += gz;
        calib_ax += ax;
        calib_ay += ay;
      }
      if(calib_count==1024){
        calib_count++;
        calib_x >>= 10;
        calib_y >>= 10;
        calib_z >>= 10;
        calib_ax >>= 10;
        calib_ay >>= 10;
      }
    }
  }else{//前拨 正常
    calib_status = 0;
    inc_x += gx - calib_x;
    inc_y += gy - calib_y;
    
    //inc_x - ay 同号
    int32_t adjx = (((int32_t)(ay-calib_ay)<<8)-inc_x)>>12;
    inc_x += adjx;
    
    //inc_y + ax 异号
    int32_t adjy = (-((int32_t)(ax-calib_ax)<<8)-inc_y)>>12;
    inc_y += adjy;
    
    inc_z += gz - calib_z;
    inc_z += (373 - (int16_t)(time1))<<3;
  }
  
  triggerStart();

}

void print(int32_t val){
  int32_t v = val;
  if(v>0){
    Serial.print(" ");
  }
  uint8_t pad = 8;
  while(v!=0){
    pad--;
    v=v/10;
  }
  for(uint8_t i=0;i<pad;i++){
    Serial.print(" ");
  }
  Serial.print(val);
}

uint8_t active;

void loop()
{
  //3： 497 249 373 开关 -----------------------------------------------------------------------------------
  //4，2，1： 286 373 460 AER -286/174*100   *0.575
  //TODO THR
/*

   Serial.print(time1);//MODE front 250~ 375 ~ 500
   Serial.print(' ');
   Serial.print(time2);//ROLL 286 373 460
   Serial.print(' ');
   Serial.print(time3);//YAW 286 373 460
   Serial.print(' ');
   Serial.print(time4);//PITCH 286 373 460
   Serial.print(' ');
   Serial.print(time5);//THR 250~500
   Serial.print(' ');
   Serial.println(' ');


   Serial.print(" ax:");print(ax);
   Serial.print(" ay:");print(ay);
   Serial.print(" az:");print(az);
   Serial.print(" gx:");print(inc_x>>8);
   Serial.print(" gy:");print(inc_y>>8);
   Serial.print(" gz:");print(inc_z>>12);
   //Serial.print("\tt0:");Serial.print(t0); 
   //Serial.print("\tff:");Serial.print(ff);
   Serial.println();

return;
*/


  if(time1>400){//mode
    doCalib = 1;
    active = 0;
  }else if(time1>300){
    doCalib = 0;
    active = 0;
  }else{
    doCalib = 0;
    active = 1;
  }

//   Serial.print(" ax:");print(ax);
//   Serial.print(" ay:");print(ay);
//   //Serial.print("\taz:");Serial.print(az);
//   Serial.print(" gx:");print(inc_x>>8);
//   Serial.print(" gy:");print(inc_y>>8);
//   Serial.print(" gz:");print(inc_z>>12);
//   //Serial.print("\tt0:");Serial.print(t0); 
//   //Serial.print("\tff:");Serial.print(ff);
//   Serial.println();

  


  
  //uint16_t time_target;

//   if(!autoRudder){//直驱
//     int16_t v1 = (((int16_t)time1 - 286) * 100) / 174;
//     v1 = 100-v1;
//     if(v1<0)v1=0;
//     if(v1>100)v1=100;
    
//     //time_target = 125+v1*5;//125~375~625    0~500
//     time_target = 125+50+v1*4;//125~375~625    0~400
    
//     PORTD|=_BV(2);
//     uint16_t time0 = TCNT1;
//     while(TCNT1-time0<time_target);//4us / tick  0.5ms 1.5ms 2.5ms
//     PORTD&=~_BV(2);
//   }else{//2 方向舵
//     int16_t v1 = inc_z>>12;
    
//     if(v1>50){
//       v1 = 50;
//     }
//       if(v1<-50){
//       v1 = -50;
//     }
//     v1 = v1 + 50; //TODO 实装角度要取反
//     //v1 = 0;

//     //3： 249 373 497  -----------------------------------------------------------------------------------
//     //4，2，1： 286 373 460
//     //int16_t v1 = (((int16_t)time1 - 286) * 100) / 174;
//     //if(v1<0)v1=0;
//     //if(v1>100)v1=100;
    
    
//     //time_target = 125+v1*5;//125~375~625    0~500
//     time_target = 125+50+v1*4;//125~375~625    0~500
    
//     PORTD|=_BV(2);
//     uint16_t time0 = TCNT1;
//     while(TCNT1-time0<time_target);//4us / tick  0.5ms 1.5ms 2.5ms
//     PORTD&=~_BV(2);
//   }
  
//   {//3 升降舵
//     int16_t temp0 = inc_y>>12;//max+-8000 ->
    
//     //int16_t temp = (((int16_t)time2 - 286) * 100) / 174;
//     int16_t temp = 0;
//     for(uint8_t i = 0;i<8;i++){
//       temp += 373 - (int16_t)time2arr[i];//归中
//     }
//     temp>>=1;

//     int16_t v1 = -temp0-temp;
    
//     if(v1>50){
//       v1 = 50;
//     }
//       if(v1<-50){
//       v1 = -50;
//     }
//     v1 = v1 + 50;

//     //v1 = 0;

//     //time_target = 125+v1*5;//125~375~625    0~500
//     time_target = 125+50+v1*4;//125~375~625    0~500
    
//     PORTD|=_BV(3);
//     uint16_t time0 = TCNT1;
//     while(TCNT1-time0<time_target);//4us / tick  0.5ms 1.5ms 2.5ms
//     PORTD&=~_BV(3);
    
//   }
  
  
 
//   {//4 副翼
//     int16_t temp0 = inc_x>>12;//max+-8000 ->
    
//     //int16_t temp = (((int16_t)time2 - 286) * 100) / 174;
//     int16_t temp = 0;
//     for(uint8_t i = 0;i<8;i++){
//       temp += 373 - (int16_t)time4arr[i];//归中
//     }
//     temp>>=1;
//     int16_t v1 = temp0-temp;
    
//     if(v1>50){
//       v1 = 50;
//     }
//       if(v1<-50){
//       v1 = -50;
//     }
//     v1 = v1 + 50;

//     //v1 = 0;
    
//     //time_target = 125+v1*5;//125~375~625    0~500
//     time_target = 125+50+v1*4;//125~375~625    0~500
    
//     PORTD|=_BV(4);
//     uint16_t time0 = TCNT1;
//     while(TCNT1-time0<time_target);//4us / tick  0.5ms 1.5ms 2.5ms
//     PORTD&=~_BV(4);
    
//   }

  //TODO 重力负反馈
  float throttle_setpoint = time5;
  throttle_setpoint = (throttle_setpoint - 250)*0.4;//0~100  THR 250~500

  float pid_output_pitch = time4;//+-50
  pid_output_pitch = (pid_output_pitch - 373)*0.57;//PITCH 286 373 460
  
  
  float pid_output_roll = time2;//+-50
  pid_output_roll = (pid_output_roll - 373)*0.57;//PITCH 286 373 460
    
  float pid_output_yaw = time3;//+-50
  pid_output_yaw = (pid_output_yaw - 373)*0.57;//PITCH 286 373 460
  
// 1  3
// 4  2
//   esc_1 = throttle_setpoint - pid_output_pitch + pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 1 (front-left - CW)
//   esc_2 = throttle_setpoint + pid_output_pitch - pid_output_roll + pid_output_yaw; //Calculate the pulse for esc 2 (rear-right - CW)
//   esc_3 = throttle_setpoint - pid_output_pitch - pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 3 (front-right - CCW)
//   esc_4 = throttle_setpoint + pid_output_pitch + pid_output_roll - pid_output_yaw; //Calculate the pulse for esc 4 (rear-left - CCW)
//   1
// 4   3
//   2
  float esc_1 = throttle_setpoint - pid_output_pitch + pid_output_yaw; //Calculate the pulse for esc 1 (front - CW)  0 ~ 100
  float esc_2 = throttle_setpoint + pid_output_pitch + pid_output_yaw; //Calculate the pulse for esc 2 (rear - CW)
  float esc_3 = throttle_setpoint + pid_output_roll  - pid_output_yaw; //Calculate the pulse for esc 3 (left - CCW)
  float esc_4 = throttle_setpoint - pid_output_roll  - pid_output_yaw; //Calculate the pulse for esc 4 (right - CCW)

  if(esc_1>100)esc_1=100;
  if(esc_2>100)esc_2=100;
  if(esc_3>100)esc_3=100;
  if(esc_4>100)esc_4=100;
  if(esc_1<0)esc_1=0;
  if(esc_2<0)esc_2=0;
  if(esc_3<0)esc_3=0;
  if(esc_4<0)esc_4=0;

  uint16_t pw_1;
  uint16_t pw_2;
  uint16_t pw_3;
  uint16_t pw_4;

  pw_1 = 240;
  pw_2 = 220;
  pw_3 = 220;
  pw_4 = 220;
  if(active){
    pw_1 += esc_1;
    pw_2 += esc_2;
    pw_3 += esc_3;
    pw_4 += esc_4;
  }
/*
   Serial.print(" 1:");print(throttle_setpoint);
   Serial.print(" 2:");print(pw_2);
   Serial.print(" 3:");print(pw_3);
   Serial.print(" 4:");print(pw_4);
   Serial.println();
*/

    PORTD|=_BV(4);
    PORTD|=_BV(5);
    PORTD|=_BV(6);
    PORTD|=_BV(7);
    uint16_t time0 = TCNT1;
    while(TCNT1-time0<2000){
      if(TCNT1-time0>pw_1)PORTD&=~_BV(4);else PORTD|=_BV(4);
      if(TCNT1-time0>pw_2)PORTD&=~_BV(5);else PORTD|=_BV(5);
      if(TCNT1-time0>pw_3)PORTD&=~_BV(6);else PORTD|=_BV(6);
      if(TCNT1-time0>pw_4)PORTD&=~_BV(7);else PORTD|=_BV(7);
    }
    
    
    /*
  {
    PORTD|=_BV(4);
    uint16_t time0 = TCNT1;
    while(TCNT1-time0<pw_1);//4us / tick  0.5ms 1.5ms 2.5ms
    PORTD&=~_BV(4);
  }
  {
    PORTD|=_BV(5);
    uint16_t time0 = TCNT1;
    while(TCNT1-time0<pw_2);//4us / tick  0.5ms 1.5ms 2.5ms
    PORTD&=~_BV(5);
  }
  {
    PORTD|=_BV(6);
    uint16_t time0 = TCNT1;
    while(TCNT1-time0<pw_3);//4us / tick  0.5ms 1.5ms 2.5ms
    PORTD&=~_BV(6);
  }
  {
    PORTD|=_BV(7);
    uint16_t time0 = TCNT1;
    while(TCNT1-time0<pw_4);//4us / tick  0.5ms 1.5ms 2.5ms
    PORTD&=~_BV(7);
  }
  */
  
  //16000max
  //a/g:	7380（俯仰 前-后+）	12820(横滚 左-右+)	-7776（无用）	-168(横滚 左-右+)	-818（俯仰 前+后-）	-755（方位 左+右-）
  
  //32767 = 500 °/s
  //1000 sample/s
  //32767*1000 = 500 deg
  //32767*1000/500 = 1 deg
  //65534=1deg
  
  //螺旋桨
  //12.7
  //15.7max
}

